/*******************************************************************************
* File Name          : LSM303DLHC.c
* Author             : Jon Henry
* Version            : $Revision:$
* Date               : $Date:$
* Description        : LSM303DLHC driver file
*                    : I2C protocols based on Standard Peripherals
*                    : Library Version 1.1.0
*                      
* HISTORY:
* Date               | Modification                    | Author
*
*******************************************************************************/

/* Includes ------------------------------------------------------------------*/
#include "lsm303dlhc.h"

/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
Gain_t MagGain;
/* Private function prototypes -----------------------------------------------*/

/******************************************************************************
* Function Name		: LSM303DLHC_ReadReg
* Description		: Reads one byte from register through I2C bus.
* Input				: Register Address
* Input				: Variable to read data to
* Output			: Data Read
* Return			: MEMS_SUCCESS
*******************************************************************************
Master |ST|   | SAD + W |   | REG |     |SR|   | SAD + R |   |   |    |NMAK|SP|
Slave     |EV5|         |EV6|     | EV8 |  |EV5|         |EV6|EV7|DATA|    |  |
*******************************************************************************
*           Transmitter Mode                         Receiver Mode            *
*     SAD + W = Slave address|0x00           SAD + R = Slave Address|0x01     *
******************************************************************************/
status_t LSM303DLHC_ReadReg(u8_t DevAddress, u8_t Reg, u8_t* Data) {

	//Wait until I2C1 is not busy any more
	while(I2C_GetFlagStatus(I2C1, I2C_FLAG_BUSY));

	//Re-enable acknowledge
	I2C_AcknowledgeConfig(I2C1, ENABLE);

	//Send I2C1 START condition
	I2C_GenerateSTART(I2C1, ENABLE);
	//Wait for I2C1 EV5 --> Slave has acknowledged start condition
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT));

	//Send slave Address for write
	I2C_Send7bitAddress(I2C1, DevAddress, I2C_Direction_Transmitter);
	//Wait for I2C1 EV6, check if slave has acknowledged Master transmitter
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));

	//Send address of register
	I2C_SendData(I2C1, Reg);
	//Wait for I2C1 EV8, check if slave has acknowledged Master Transmitter
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTING));

	//Send START condition a second time (Re-Start)
	I2C_GenerateSTART(I2C1, ENABLE);
	//Wait for I2C1 EV5 --> Slave has acknowledged start condition
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT));

	//Send slave Address for read
	I2C_Send7bitAddress(I2C1, DevAddress, I2C_Direction_Receiver);

	//Send NMAK
	I2C_AcknowledgeConfig(I2C1, DISABLE);

	//Wait for I2C1 EV6, check if slave has acknowledged Master Receiver
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED));

	//Wait for I2C1 EV7 --> One byte has been received
	while( !I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED) );

	//Read data from I2C data register and return data byte
	*Data = I2C_ReceiveData(I2C1);

	I2C_GenerateSTOP(I2C1, ENABLE);
	//stop bit flag
	while(I2C_GetFlagStatus(I2C1, I2C_FLAG_STOPF));

  return MEMS_SUCCESS;
}

/******************************************************************************
* Function Name		: LSM303DLHC_ReadBuffer
* Description		: Generic Reading function. It must be fulfilled with either
*					: I2C or SPI reading functions
* Input				: Starting Register Address
* Output			: Data Buffer Read
* Return			: None
*******************************************************************************
Master|ST| |SAD + W| |REG|     |SR| |SAD + R|   |   |MAK |   |MAK |   |NMAK|SP|
Slave   |EV5|     |EV6|  |EV8_2| |EV5|      |EV6|EV7|DATA|   |DATA|   |DATA|
*******************************************************************************
*           Transmitter Mode                         Receiver Mode            *
*     SAD + W = Slave address|0x00           SAD + R = Slave Address|0x01     *
******************************************************************************/
status_t LSM303DLHC_ReadBuffer(u8_t DevAddress, uint8_t Reg, AxesRaw_t* buff) {

	u8 XMSB,XLSB,YMSB,YLSB,ZMSB,ZLSB;

	//Test on BUSY Flag
	while (I2C_GetFlagStatus(I2C1,I2C_FLAG_BUSY));

	//Re-enable acknowledge
	I2C_AcknowledgeConfig(I2C1, ENABLE);

	//Send I2C1 START condition
	I2C_GenerateSTART(I2C1, ENABLE);
	//Wait for I2C1 EV5 --> Slave has acknowledged start condition
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT));

	//Send device address for write
	I2C_Send7bitAddress(I2C1, DevAddress, I2C_Direction_Transmitter);
	//Wait for I2C1 EV6, check if slave has acknowledged Master transmitter
	while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));

	//Send the device's internal address to read from with MSB asserted
	I2C_SendData(I2C1,Reg|0x80);
	//Wait for I2C1 EV8_2, check if slave has acknowledged Master Transmitter
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTING));

	//Send START condition a second time (Re-Start)
	I2C_GenerateSTART(I2C1, ENABLE);
	//Wait for I2C1 EV5 --> Slave has acknowledged start condition
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT));

	//Send address for read
	I2C_Send7bitAddress(I2C1, DevAddress, I2C_Direction_Receiver);
	//Wait for I2C1 EV6, check if slave has acknowledged Master Receiver
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED));

	if(DevAddress == ACC_I2C_ADDRESS)
	{
		//Load in all 6 registers */
		while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
		XMSB = I2C_ReceiveData(I2C1);
		while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
		XLSB = I2C_ReceiveData(I2C1);
		while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
		YMSB = I2C_ReceiveData(I2C1);
		while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
		YLSB = I2C_ReceiveData(I2C1);
		while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
		ZMSB = I2C_ReceiveData(I2C1);
		//Disable ACK after last byte read and before actually reading it
		I2C_AcknowledgeConfig(I2C1, DISABLE);
		while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
		I2C_GenerateSTOP(I2C1, ENABLE);
		ZLSB = I2C_ReceiveData(I2C1);
		//Ensure stop bit was set.
		while(I2C_GetFlagStatus(I2C1, I2C_FLAG_STOPF));

		//Sort into 3 global variables
		buff->AXIS_X = -(i16_t)((XMSB<<8) | XLSB);
		buff->AXIS_Y = -(i16_t)((YMSB<<8) | YLSB);
		buff->AXIS_Z = -(i16_t)((ZMSB<<8) | ZLSB);
	}
	else if(DevAddress == MAG_I2C_ADDRESS)
	{
		//Load in all 6 registers */
		while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
		XMSB = I2C_ReceiveData(I2C1);
		while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
		XLSB = I2C_ReceiveData(I2C1);
		while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
		ZMSB = I2C_ReceiveData(I2C1);
		while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
		ZLSB = I2C_ReceiveData(I2C1);
		while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
		YMSB = I2C_ReceiveData(I2C1);
		//Disable ACK after last byte read and before actually reading it
		I2C_AcknowledgeConfig(I2C1, DISABLE);
		while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
		I2C_GenerateSTOP(I2C1, ENABLE);
		YLSB = I2C_ReceiveData(I2C1);
		//Ensure stop bit was set.
		while(I2C_GetFlagStatus(I2C1, I2C_FLAG_STOPF));

		//Sort into 3 global variables
		buff->AXIS_X = (i16_t)((XMSB<<8) | XLSB);
		buff->AXIS_Y = -(i16_t)((YMSB<<8) | YLSB);
		buff->AXIS_Z = -(i16_t)((ZMSB<<8) | ZLSB);
	}

	return MEMS_SUCCESS;
}

/*******************************************************************************
* Function Name		: LSM303DLHC_WriteReg
* Description		: Generic write function.
* Input				: Register Address, Data to be written
* Output			: None
* Return			: None
*******************************************************************************
Master |ST|         |SAD + W|           |REG|           |DATA|             |SP|
Slave        |EV5|              |EV6|          |EV8_2|            |EV5|
*******************************************************************************
*           Transmitter Mode                         Receiver Mode            *
*     SAD + W = Slave address|0x00           SAD + R = Slave Address|0x01     *
******************************************************************************/
status_t LSM303DLHC_WriteReg(u8_t DevAddress, u8_t Reg, u8_t Data) {

	//I2C Write Sequence

	//Wait until I2C1 is not busy anymore
	while(I2C_GetFlagStatus(I2C1, I2C_FLAG_BUSY));

	// Send I2C1 START condition
	I2C_GenerateSTART(I2C1, ENABLE);
	//Wait for I2C1 EV5 --> Slave has acknowledged start condition
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT));

	//Send slave Address for write
	I2C_Send7bitAddress(I2C1, DevAddress, I2C_Direction_Transmitter);
	//Wait for I2C1 EV6, check if slave has acknowledged Master transmitter
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));

	//Mode register address
	I2C_SendData(I2C1, Reg);
	//wait for byte send to complete
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED));

	//wait for byte send to complete
	while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED));
	//clear bits
	I2C_SendData(I2C1, Data);

	//generate stop
	I2C_GenerateSTOP(I2C1, ENABLE);
	//stop bit flag
	while(I2C_GetFlagStatus(I2C1, I2C_FLAG_STOPF));

	return MEMS_SUCCESS;
}


/* Private functions ---------------------------------------------------------*/

/*******************************************************************************
* Function Name  : LSM303DLHC_Init
* Description    : Sets LSM303DLHC configuration
* Input          : None
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_Init(){

	LSM303DLHC_SetODR(ODR_100Hz);
	LSM303DLHC_SetMode(NORMAL);
	LSM303DLHC_SetAxis(X_ENABLE | Y_ENABLE | Z_ENABLE);
	LSM303DLHC_SetFullScale(FULLSCALE_2);
	LSM303DLHC_SetBDU(MEMS_DISABLE);
	LSM303DLHC_SetBLE(BLE_MSB);
	LSM303DLHC_SetSelfTest(SELF_TEST_DISABLE);
	LSM303DLHC_SetTemperature(MEMS_DISABLE);

	LSM303DLHC_SetODR_M(ODR_75Hz_M);
	LSM303DLHC_SetGainMag(GAIN_1100_M);
	LSM303DLHC_SetModeMag(CONTINUOUS_MODE);

 return MEMS_SUCCESS;
}

/********************************************************************************
* Function Name  : SetODR
* Description    : Sets LSM303DLHC Accelerometer Output Data Rate via CTRL_REG1_A
* Input          : Output Data Rate
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*********************************************************************************
*   ODR3  |   ODR2  |   ODR1  |   ODR0  |   LPen  |   Zen   |   Yen   |   Xen   | 
*    0    |    0    |    0    |    0    |    0    |    1    |    1    |    1    |
*********************************************************************************
* ODR3| ODR2| ODR1| ODR0|  POWER MODE SELECTION  |
*  0  |  0  |  0  |     |       POWER DOWN       |
*  0  |  0  |  1  |     |  NORM/LOW POWER(1Hz)   |
*  0  |  1  |  0  |     |  NORM/LOW POWER(10Hz)  |
*  0  |  1  |  1  |     |  NORM/LOW POWER(25Hz)  |
*  1  |  0  |  0  |     |  NORM/LOW POWER(50Hz)  |
*  1  |  0  |  1  |     |  NORM/LOW POWER(100Hz) |
*  1  |  1  |  0  |     |  NORM/LOW POWER(200Hz) |
*  1  |  1  |  1  |     |  NORM/LOW POWER(400Hz) |
*  1  |  1  |  1  |     |  NORM/LOW POWER(400Hz) |
*  1  |  1  |  1  |     |  LOW POWER (1.620KHz)  |
*  1  |  1  |  1  |     | NORM(1.344)/LOW(5.376) |
********************************************************************************/
status_t LSM303DLHC_SetODR(ODR_t ov){
 u8_t value;
 u8_t mask = 0x0F;

 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, CTRL_REG1_A, &value) )
   return MEMS_ERROR;

 value = (value & mask) | (ov << 4);

 if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, CTRL_REG1_A, value) )
   return MEMS_ERROR;

 return MEMS_SUCCESS;
}

/********************************************************************************
* Function Name  : SetODR_M
* Description    : Sets LSM303DLHC Magnetometer Output Data Rate via CRA_REG_M
* Input          : Output Data Rate
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*********************************************************************************
* TEMP_EN |   (0)   |   (0)   |   DO2   |   DO1   |   DO0   |   (0)   |   (0)   | 
*    0    |    0    |    0    |    1    |    0    |    0    |    0    |    0    |
*********************************************************************************
* DO2 | DO1 | DO0 | MINIMUM DATA OUTPUT RATE |
*  0  |  0  |  0  |          0.75 Hz         |
*  0  |  0  |  1  |          1.5  Hz         |
*  0  |  1  |  0  |          3.0  Hz         | 
*  0  |  1  |  1  |          7.5  Hz         | 
*  1  |  0  |  0  |         15.0  Hz         | 
*  1  |  0  |  1  |         30.0  Hz         | 
*  1  |  1  |  0  |         75.0  Hz         | 
*  1  |  1  |  1  |        220.0  Hz         | 
********************************************************************************/
status_t LSM303DLHC_SetODR_M(ODR_M_t ov){
 u8_t value;
 u8_t mask = 0x63;

 if( !LSM303DLHC_ReadReg(MAG_I2C_ADDRESS, CRA_REG_M, &value) )
   return MEMS_ERROR;

 value = (value & mask) | (ov << ODR_M);

 if( !LSM303DLHC_WriteReg(MAG_I2C_ADDRESS, CRA_REG_M, value) )
   return MEMS_ERROR;

 return MEMS_SUCCESS;
}

/********************************************************************************
* Function Name  : SetTemperature
* Description    : Sets LSM303DLHC Output Temperature Status via CRA_REG_M
* Input          : MEMS_ENABLE, MEMS_DISABLE
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*********************************************************************************
* TEMP_EN |   (0)   |   (0)   |   DO2   |   DO1   |   DO0   |   (0)   |   (0)   | 
*    0    |    0    |    0    |    1    |    0    |    0    |    0    |    0    |
*********************************************************************************
*  TEMP_EN  |  Temperature Sensor Enable:   0-> Disabled (Default)    1-> Enabled
********************************************************************************/
status_t LSM303DLHC_SetTemperature(State_t state){
 u8_t value;
 u8_t mask = 0xEF;

 if( !LSM303DLHC_ReadReg(MAG_I2C_ADDRESS, CRA_REG_M, &value) )
   return MEMS_ERROR;

 value = (value & mask) | (state << TEMP_EN);

 if( !LSM303DLHC_WriteReg(MAG_I2C_ADDRESS, CRA_REG_M, value) )
   return MEMS_ERROR;

 return MEMS_SUCCESS;
}


/********************************************************************************
* Function Name  : SetGainMag
* Description    : Sets LSM303DLHC Magnetometer Gain
* Input          : GAIN_1100_M or GAIN_855_M or GAIN_670_M or GAIN_450_M....
* Output         : MagGain.XY and MagGain.Z are set
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*********************************************************************************
*   GN2   |   GN1   |   GN0   |   (0)   |   (0)   |   (0)   |   (0)   |   (0)   | 
*    0    |    0    |    1    |    0    |    0    |    0    |    0    |    0    |
*********************************************************************************
* GN2 | GN1 | GN0 | SENSOR INPUT RANGE | X, Y Gain | Z Gain |
*  0  |  0  |  1  |      +-1.3         |   1100    |   980  |
*  0  |  1  |  0  |      +-1.9         |    855    |   780  |
*  0  |  1  |  1  |      +-2.5         |    670    |   600  |
*  1  |  0  |  0  |      +-4.0         |    450    |   400  |
*  1  |  0  |  1  |      +-4.7         |    400    |   355  |
*  1  |  1  |  0  |      +-5.6         |    330    |   295  |
*  1  |  1  |  1  |      +-8.1         |    230    |   205  |
********************************************************************************/
status_t LSM303DLHC_SetGainMag(GAIN_M_t Gain){
 u8_t value;
 //u8_t mask = 0x1F;

 if( !LSM303DLHC_ReadReg(MAG_I2C_ADDRESS, CRB_REG_M, &value) )
   return MEMS_ERROR;

 switch(value)
 {
 	 case 0x20:
 		 MagGain.XY = 1100;
 		 MagGain.Z = 980;
 		 break;
 	 case 0x40:
 		 MagGain.XY = 855;
 		 MagGain.Z = 760;
 		 break;
 	 case 0x60:
 	 	 MagGain.XY = 670;
 	 	 MagGain.Z = 600;
 	 	 break;
 	 case 0x80:
 	 	 MagGain.XY = 450;
 	 	 MagGain.Z = 400;
 	 	 break;
 	 case 0xA0:
 	 	 MagGain.XY = 400;
 	 	 MagGain.Z = 355;
 	 	 break;
 	 case 0xC0:
 	 	 MagGain.XY = 330;
 	 	 MagGain.Z = 295;
 	 	 break;
 	 case 0xE0:
 	 	 MagGain.XY = 230;
 	 	 MagGain.Z = 205;
 	 	 break;
 }
	//value = (value & mask) | (Gain << GN_CFG);

 if( !LSM303DLHC_WriteReg(MAG_I2C_ADDRESS, CRB_REG_M, value) )
   return MEMS_ERROR;

 return MEMS_SUCCESS;
}

/********************************************************************************
* Function Name  : SetModeMag
* Description    : Sets LSM303DLHC Magnetometer Modality
* Input          : Modality (CONTINUOUS_MODE, or SINGLE_MODE, or SLEEP_MODE)
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*********************************************************************************
*   (0)   |   (0)   |   (0)   |   (0)   |   (0)   |   (0)   |   MD1   |   MD0   | 
*    0    |    0    |    0    |    0    |    0    |    0    |    1    |    1    |
*********************************************************************************
* MD1 | MD0 | MAGNETIC SENSOR OPERATING MODE
*  0  |  0  | CONTINUOUS CONVERSION MODE
*  0  |  1  | SINGLE CONVERSION MODE
*  1  |  0  | SLEEP MODE
*  1  |  1  | SLEEP MODE
********************************************************************************/
status_t LSM303DLHC_SetModeMag(Mode_M_t Mode){

 if( !LSM303DLHC_WriteReg(MAG_I2C_ADDRESS, MR_REG_M, Mode) )
   return MEMS_ERROR;

 return MEMS_SUCCESS;
}

/********************************************************************************
* Function Name  : GetTempRaw
* Description    : Read the Temperature Values by TEMP_OUT Output Registers
* Input          : Value to empty (16 Bit two's complement)
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
********************************************************************************/
status_t LSM303DLHC_GetTempRaw(i16_t* val) {
 u8_t valueH;
 
 if( !LSM303DLHC_ReadReg(MAG_I2C_ADDRESS, TEMP_OUT_H_M, &valueH) )
     return MEMS_ERROR;
 
 *val = 25 + (i16_t)(valueH);
 
 return MEMS_SUCCESS;  
}

/********************************************************************************
* Function Name  : SetMode
* Description    : Sets LSM303DLHC Operating Mode Accelerometer
* Input          : Modality (NORMAL, LOW_POWER, POWER_DOWN)
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
********************************************************************************/
status_t LSM303DLHC_SetMode(Mode_t md) {
 u8_t value;
 u8_t value2;
 static   u8_t ODR_old_value;

 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, CTRL_REG1_A, &value) )
   return MEMS_ERROR;
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, CTRL_REG4_A, &value2) )
   return MEMS_ERROR;
 
 if((value & 0xF0)==0) value = value | (ODR_old_value & 0xF0); //if it comes from POWERDOWN  
   
 switch(md) {
 
 case POWER_DOWN:
   ODR_old_value = value;
   value &= 0x0F;
   break;
         
 case NORMAL:
   value &= 0xF7;
   value |= (MEMS_RESET<<LPEN);
   value2 &= 0xF7;
   value2 |= (MEMS_SET<<HR);   //set HighResolution_BIT
   break;
         
 case LOW_POWER:
   value &= 0xF7;
   value |=  (MEMS_SET<<LPEN);
   value2 &= 0xF7;
   value2 |= (MEMS_RESET<<HR); //reset HighResolution_BIT
   break;
         
 default:
   return MEMS_ERROR;
 }
 
 if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, CTRL_REG1_A, value) )
   return MEMS_ERROR;
 
 if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, CTRL_REG4_A, value2) )
   return MEMS_ERROR;  
 
 return MEMS_SUCCESS;
}

/*******************************************************************************
* Function Name  : SetAxis
* Description    : Enable/Disable LSM303DLHC Axis
* Input          : X_ENABLE/X_DISABLE | Y_ENABLE/Y_DISABLE | Z_ENABLE/Z_DISABLE
* Output         : None
* Note           : You MUST use all input variable in the argument, as example
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*********************************************************************************
*   ODR3  |   ODR2  |   ODR1  |   ODR0  |   LPen  |   Zen   |   Yen   |   Xen   | 
*    0    |    0    |    0    |    0    |    0    |    1    |    1    |    1    |
*********************************************************************************
*   Zen    |  Z Axis Enable:   0-> Disabled       1-> Enabled (Default)
*   Yen    |  Y Axis Enable:   0-> Disabled       1-> Enabled (Default)
*   Xen    |  X Axis Enable:   0-> Disabled       1-> Enabled (Default)
********************************************************************************/
status_t LSM303DLHC_SetAxis(Axis_t axis) {
 u8_t value;
 u8_t mask = 0XF8;
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, CTRL_REG1_A, &value) )
   return MEMS_ERROR;

 value = (value & mask) | axis;
 
 if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, CTRL_REG1_A, value) )
   return MEMS_ERROR;  
 
 return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : SetFullScale
* Description    : Sets the LSM303DLHC FullScale
* Input          : FULLSCALE_2/FULLSCALE_4/FULLSCALE_8/FULLSCALE_16
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*********************************************************************************
*   BDU   |   BLE   |   FS1   |   FS0   |    HR   |   (0)   |   (0)   |   SIM   | 
*    0    |    0    |    0    |    0    |    0    |    0    |    1    |    1    |
*********************************************************************************
* FS1 | FS0 | FULL-SCALE SELECTION
*  0  |  0  | +-2g
*  0  |  1  | +-4g
*  1  |  0  | +-8g
*  1  |  1  | +-16g
********************************************************************************/
status_t LSM303DLHC_SetFullScale(Fullscale_t fs) {
 u8_t value;
 u8_t mask = 0xCF;
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, CTRL_REG4_A, &value) )
   return MEMS_ERROR;
                 
 value = (value & mask) | (fs << FS);
 
 if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, CTRL_REG4_A, value) )
   return MEMS_ERROR;
 
 return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : SetBDU
* Description    : Enable/Disable Block Data Update Functionality
* Input          : ENABLE/DISABLE
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*********************************************************************************
*   BDU   |   BLE   |   FS1   |   FS0   |    HR   |   (0)   |   (0)   |   SIM   | 
*    0    |    0    |    0    |    0    |    0    |    0    |    0    |    0    |
*********************************************************************************
*  BDU  | Block Update:  0->Continuous  1->Registers updated after MSB & LSB read
********************************************************************************/
status_t LSM303DLHC_SetBDU(State_t bdu) {
 u8_t value;
 u8_t mask = 0xEF;
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, CTRL_REG4_A, &value) )
   return MEMS_ERROR;

 value = (value & mask) | (bdu << BDU);

 if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, CTRL_REG4_A, value) )
   return MEMS_ERROR;

 return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : SetBLE
* Description    : Set Endianess (MSB/LSB)
* Input          : BLE_LSB / BLE_MSB
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*********************************************************************************
*   BDU   |   BLE   |   FS1   |   FS0   |    HR   |   (0)   |   (0)   |   SIM   | 
*    0    |    0    |    0    |    0    |    0    |    0    |    0    |    0    |
*********************************************************************************
*   BLE   | Big/Little Endian: 0-> LSB at lower Address  1-> MSB at lower Address
********************************************************************************/
status_t LSM303DLHC_SetBLE(Endianess_t ble) {
 u8_t value;
 u8_t mask = 0xCF;
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, CTRL_REG4_A, &value) )
   return MEMS_ERROR;
                 
 value = (value & mask) | (ble << BLE);
 
 if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, CTRL_REG4_A, value) )
   return MEMS_ERROR;
 
 return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : SetSelfTest
* Description    : Set Self Test Modality
* Input          : SELF_TEST_DISABLE/ST_0/ST_1
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_SetSelfTest(SelfTest_t st) {
 u8_t value;
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, CTRL_REG4_A, &value) )
   return MEMS_ERROR;
                 
 value &= 0xF9;
 value |= (st<<ST);
 
 if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, CTRL_REG4_A, value) )
   return MEMS_ERROR;
 
 return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : HPFClick
* Description    : Enable/Disable High Pass Filter for click
* Input          : MEMS_ENABLE/MEMS_DISABLE
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_HPFClickEnable(State_t hpfe) {
 u8_t value;
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, CTRL_REG2_A, &value) )
   return MEMS_ERROR;
                 
 value &= 0xFB;
 value |= (hpfe<<HPCLICK);
 
 if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, CTRL_REG2_A, value) )
   return MEMS_ERROR;
 
 return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : HPFAOI1
* Description    : Enable/Disable High Pass Filter for AOI on INT_1
* Input          : MEMS_ENABLE/MEMS_DISABLE
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_HPFAOI1Enable(State_t hpfe) {
 u8_t value;
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, CTRL_REG2_A, &value) )
   return MEMS_ERROR;
                 
 value &= 0xFE;
 value |= (hpfe<<HPIS1);
 
 if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, CTRL_REG2_A, value) )
   return MEMS_ERROR;
 
 return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : HPFAOI2
* Description    : Enable/Disable High Pass Filter for AOI on INT_2
* Input          : MEMS_ENABLE/MEMS_DISABLE
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_HPFAOI2Enable(State_t hpfe) {
 u8_t value;
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, CTRL_REG2_A, &value) )
   return MEMS_ERROR;
                 
 value &= 0xFD;
 value |= (hpfe<<HPIS2);
 
 if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, CTRL_REG2_A, value) )
   return MEMS_ERROR;
 
 return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : SetHPFMode
* Description    : Set High Pass Filter Modality
* Input          : HPM_NORMAL_MODE_RES/HPM_REF_SIGNAL/HPM_NORMAL_MODE/HPM_AUTORESET_INT
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_SetHPFMode(HPFMode_t hpm) {
 u8_t value;
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, CTRL_REG2_A, &value) )
   return MEMS_ERROR;
                 
 value &= 0x3F;
 value |= (hpm<<HPM);
 
 if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, CTRL_REG2_A, value) )
   return MEMS_ERROR;
 
 return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : SetHPFCutOFF
* Description    : Set High Pass CUT OFF Freq
* Input          : HPFCF [0,3]
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_SetHPFCutOFF(HPFCutOffFreq_t hpf) {
 u8_t value;
   
 if (hpf > 3)
   return MEMS_ERROR;
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, CTRL_REG2_A, &value) )
   return MEMS_ERROR;
                 
 value &= 0xCF;
 value |= (hpf<<HPCF);
 
 if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, CTRL_REG2_A, value) )
   return MEMS_ERROR;
 
 return MEMS_SUCCESS;
 
}


/*******************************************************************************
* Function Name  : SetFilterDataSel
* Description    : Set Filter Data Selection bypassed or sent to FIFO OUT register
* Input          : MEMS_SET, MEMS_RESET
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_SetFilterDataSel(State_t state) {
 u8_t value;
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, CTRL_REG2_A, &value) )
   return MEMS_ERROR;
                 
 value &= 0xF7;
 value |= (state<<FDS);
 
 if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, CTRL_REG2_A, value) )
   return MEMS_ERROR;
 
 return MEMS_SUCCESS;
 
}


/*******************************************************************************
* Function Name  : SetInt1Pin
* Description    : Set Interrupt1 pin Function
* Input          :  CLICK_ON_PIN_INT1_ENABLE/DISABLE    | I1_INT1_ON_PIN_INT1_ENABLE/DISABLE |              
                   I1_INT2_ON_PIN_INT1_ENABLE/DISABLE  | I1_DRDY1_ON_INT1_ENABLE/DISABLE    |              
                   I1_DRDY2_ON_INT1_ENABLE/DISABLE     | WTM_ON_INT1_ENABLE/DISABLE         |          
                   INT1_OVERRUN_ENABLE/DISABLE  
* example        : SetInt1Pin(CLICK_ON_PIN_INT1_ENABLE | I1_INT1_ON_PIN_INT1_ENABLE |              
                   I1_INT2_ON_PIN_INT1_DISABLE | I1_DRDY1_ON_INT1_ENABLE | I1_DRDY2_ON_INT1_ENABLE     |
                   WTM_ON_INT1_DISABLE | INT1_OVERRUN_DISABLE   )
* Note           : To enable Interrupt signals on INT1 Pad (You MUST use all input variable in the argument, as example)
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_SetInt1Pin(IntPinConf_t pinConf) {
 u8_t value;
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, CTRL_REG3_A, &value) )
   return MEMS_ERROR;
                 
 value &= 0x00;
 value |= pinConf;
 
 if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, CTRL_REG3_A, value) )
   return MEMS_ERROR;
 
 return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : SetInt2Pin
* Description    : Set Interrupt2 pin Function
* Input          : CLICK_ON_PIN_INT2_ENABLE/DISABLE   | I2_INT1_ON_PIN_INT2_ENABLE/DISABLE |              
                  I2_INT2_ON_PIN_INT2_ENABLE/DISABLE | I2_BOOT_ON_INT2_ENABLE/DISABLE |                  
                  INT_ACTIVE_HIGH/LOW
* example        : SetInt2Pin(CLICK_ON_PIN_INT2_ENABLE/DISABLE | I2_INT1_ON_PIN_INT2_ENABLE/DISABLE |              
                  I2_INT2_ON_PIN_INT2_ENABLE/DISABLE | I2_BOOT_ON_INT2_ENABLE/DISABLE |                  
                  INT_ACTIVE_HIGH/LOW)
* Note           : To enable Interrupt signals on INT2 Pad (You MUST use all input variable in the argument, as example)
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_SetInt2Pin(IntPinConf_t pinConf) {
 u8_t value;
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, CTRL_REG6_A, &value) )
   return MEMS_ERROR;
                 
 value &= 0x00;
 value |= pinConf;
 
 if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, CTRL_REG6_A, value) )
   return MEMS_ERROR;
 
 return MEMS_SUCCESS;
}                      


/*******************************************************************************
* Function Name  : SetClickCFG
* Description    : Set Click Interrupt config Function
* Input          : ZD_ENABLE/DISABLE | ZS_ENABLE/DISABLE  | YD_ENABLE/DISABLE  |
                  YS_ENABLE/DISABLE | XD_ENABLE/DISABLE  | XS_ENABLE/DISABLE
* example        : SetClickCFG( ZD_ENABLE | ZS_DISABLE | YD_ENABLE |
                              YS_DISABLE | XD_ENABLE | XS_ENABLE)
* Note           : You MUST use all input variable in the argument, as example
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_SetClickCFG(u8_t status) {
 u8_t value;
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, CLICK_CFG_A, &value) )
   return MEMS_ERROR;
                 
 value &= 0xC0;
 value |= status;
 
 if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, CLICK_CFG_A, value) )
   return MEMS_ERROR;
 
 return MEMS_SUCCESS;
}  


/*******************************************************************************
* Function Name  : SetClickTHS
* Description    : Set Click Interrupt threshold
* Input          : Click-click Threshold value [0-127]
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_SetClickTHS(u8_t ths) {
 
 if(ths>127)     return MEMS_ERROR;
 
 if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, CLICK_THS_A, ths) )
   return MEMS_ERROR;
 
 return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : SetClickLIMIT
* Description    : Set Click Interrupt Time Limit
* Input          : Click-click Time Limit value [0-127]
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_SetClickLIMIT(u8_t t_limit) {
 
 if(t_limit>127)     return MEMS_ERROR;
 
 if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, TIME_LIMIT_A, t_limit) )
   return MEMS_ERROR;
 
 return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : SetClickLATENCY
* Description    : Set Click Interrupt Time Latency
* Input          : Click-click Time Latency value [0-255]
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_SetClickLATENCY(u8_t t_latency) {
 
 if(t_latency>255)     return MEMS_ERROR;
 
 if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, TIME_LATENCY_A, t_latency) )
   return MEMS_ERROR;
 
 return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : SetClickWINDOW
* Description    : Set Click Interrupt Time Window
* Input          : Click-click Time Window value [0-255]
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_SetClickWINDOW(u8_t t_window) {
 
 if(t_window>255)     return MEMS_ERROR;
 
 if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, TIME_WINDOW_A, t_window) )
   return MEMS_ERROR;
 
 return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : GetClickResponce
* Description    : Get Click Interrupt Response by CLICK_SRC REGISTER
* Input          : char to empty by Click Response Typedef
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_GetClickResponce(u8_t* res) {
 u8_t value;
 
if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, CLICK_SRC_A, &value) )
  return MEMS_ERROR;

value &= 0x7F;

if((value & IA)==0) {        *res = NO_CLICK;     return MEMS_SUCCESS;}
else {
  if (value & DCLICK){
    if (value & CLICK_SIGN){
       if (value & CLICK_Z) {*res = DCLICK_Z_N;   return MEMS_SUCCESS;}
       if (value & CLICK_Y) {*res = DCLICK_Y_N;   return MEMS_SUCCESS;}
       if (value & CLICK_X) {*res = DCLICK_X_N;   return MEMS_SUCCESS;}
    }
    else{
       if (value & CLICK_Z) {*res = DCLICK_Z_P;   return MEMS_SUCCESS;}
       if (value & CLICK_Y) {*res = DCLICK_Y_P;   return MEMS_SUCCESS;}
       if (value & CLICK_X) {*res = DCLICK_X_P;   return MEMS_SUCCESS;}
    }      
  }
  else{
    if (value & CLICK_SIGN){
       if (value & CLICK_Z) {*res = SCLICK_Z_N;   return MEMS_SUCCESS;}
       if (value & CLICK_Y) {*res = SCLICK_Y_N;   return MEMS_SUCCESS;}
       if (value & CLICK_X) {*res = SCLICK_X_N;   return MEMS_SUCCESS;}
    }
    else{
       if (value & CLICK_Z) {*res = SCLICK_Z_P;   return MEMS_SUCCESS;}
       if (value & CLICK_Y) {*res = SCLICK_Y_P;   return MEMS_SUCCESS;}
       if (value & CLICK_X) {*res = SCLICK_X_P;   return MEMS_SUCCESS;}
    }
  }
}
return MEMS_ERROR;
}


/*******************************************************************************
* Function Name  : Int1LatchEnable
* Description    : Enable Interrupt 1 Latching function
* Input          : ENABLE/DISABLE
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_Int1LatchEnable(State_t latch) {
 u8_t value;
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, CTRL_REG5_A, &value) )
   return MEMS_ERROR;
                 
 value &= 0xF7;
 value |= latch<<LIR_INT1;
 
 if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, CTRL_REG5_A, value) )
   return MEMS_ERROR;
 
 return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : ResetInt1Latch
* Description    : Reset Interrupt 1 Latching function
* Input          : None
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_ResetInt1Latch(void) {
 u8_t value;
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, INT1_SRC_A, &value) )
   return MEMS_ERROR;
 
 return MEMS_SUCCESS;
}

     
/*******************************************************************************
* Function Name  : SetInt1Configuration
* Description    : Interrupt 1 Configuration (without 6D_INT)
* Input          : INT_AND/OR | INT_ZHIE_ENABLE/DISABLE | INT_ZLIE_ENABLE/DISABLE...
* Output         : None
* Note           : You MUST use all input variable in the argument, as example
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_SetInt1Configuration(IntConf_t ic) {
 u8_t value;
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, INT1_CFG_A, &value) )
   return MEMS_ERROR;
 
 value &= 0x40;
 value |= ic;
 
 if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, INT1_CFG_A, value) )
   return MEMS_ERROR;

 return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : SetInt2Configuration
* Description    : Interrupt 1 Configuration (without 6D_INT)
* Input          : INT_AND/OR | INT_ZHIE_ENABLE/DISABLE | INT_ZLIE_ENABLE/DISABLE...
* Output         : None
* Note           : You MUST use all input variable in the argument, as example
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_SetInt2Configuration(IntConf_t ic) {
 u8_t value;
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, INT2_CFG_A, &value) )
   return MEMS_ERROR;
 
 value &= 0x40;
 value |= ic;
 
 if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, INT2_CFG_A, value) )
   return MEMS_ERROR;

 return MEMS_SUCCESS;
}
     
   
/*******************************************************************************
* Function Name  : SetIntMode
* Description    : Interrupt 1 Configuration mode (OR, 6D Movement, AND, 6D Position)
* Input          : INT_MODE_OR, INT_MODE_6D_MOVEMENT, INT_MODE_AND, INT_MODE_6D_POSITION
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_SetIntMode(Int1Mode_t int_mode) {
 u8_t value;
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, INT1_CFG_A, &value) )
   return MEMS_ERROR;
 
 value &= 0x3F;
 value |= (int_mode<<INT_6D);
 
 if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, INT1_CFG_A, value) )
   return MEMS_ERROR;

 return MEMS_SUCCESS;
}

   
/*******************************************************************************
* Function Name  : SetInt6D4DConfiguration
* Description    : 6D, 4D Interrupt Configuration
* Input          : INT1_6D_ENABLE, INT1_4D_ENABLE, INT1_6D_4D_DISABLE
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_SetInt6D4DConfiguration(INT_6D_4D_t ic) {
 u8_t value;
 u8_t value2;
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, INT1_CFG_A, &value) )
   return MEMS_ERROR;
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, CTRL_REG5_A, &value2) )
   return MEMS_ERROR;
 
 if(ic == INT1_6D_ENABLE){
     value &= 0xBF;
     value |= (MEMS_ENABLE<<INT_6D);
     value2 &= 0xFB;
     value2 |= (MEMS_DISABLE<<D4D_INT1);
 }
 
   if(ic == INT1_4D_ENABLE){
     value &= 0xBF;
     value |= (MEMS_ENABLE<<INT_6D);
     value2 &= 0xFB;
     value2 |= (MEMS_ENABLE<<D4D_INT1);
 }
 
   if(ic == INT1_6D_4D_DISABLE){
     value &= 0xBF;
     value |= (MEMS_DISABLE<<INT_6D);
     value2 &= 0xFB;
     value2 |= (MEMS_DISABLE<<D4D_INT1);
 }
 
 if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, INT1_CFG_A, value) )
   return MEMS_ERROR;
 if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, CTRL_REG5_A, value2) )
   return MEMS_ERROR;

 return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : Get6DPosition
* Description    : 6D, 4D Interrupt Position Detect
* Input          : Byte to empty by POSITION_6D_t Typedef
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_Get6DPosition(u8_t* val){
 u8_t value;
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, INT1_SRC_A, &value) )
   return MEMS_ERROR;

 value &= 0x7F;
 
 switch (value){
 case UP_SX:   *val = UP_SX;    break;
 case UP_DX:   *val = UP_DX;    break;
 case DW_SX:   *val = DW_SX;    break;
 case DW_DX:   *val = DW_DX;    break;
 case TOP:     *val = TOP;      break;
 case BOTTOM:  *val = BOTTOM;   break;
 }
 
return MEMS_SUCCESS;  
}


/*******************************************************************************
* Function Name  : SetInt1Threshold
* Description    : Sets Interrupt 1 Threshold
* Input          : Threshold = [0,31]
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_SetInt1Threshold(u8_t ths) {
 if (ths > 127)
   return MEMS_ERROR;
 
     if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, INT1_THS_A, ths) )
       return MEMS_ERROR;    

 return MEMS_SUCCESS;
}

   
/*******************************************************************************
* Function Name  : SetInt1Duration
* Description    : Sets Interrupt 1 Duration
* Input          : Duration value
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_SetInt1Duration(IntConf_t id) {

 if (id > 127)
   return MEMS_ERROR;

 if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, INT1_DURATION_A, id) )
   return MEMS_ERROR;

 return MEMS_SUCCESS;
}
     

/*******************************************************************************
* Function Name  : FIFOModeEnable
* Description    : Sets Fifo Modality
* Input          : FIFO_DISABLE, FIFO_BYPASS_MODE, FIFO_MODE, FIFO_STREAM_MODE, FIFO_TRIGGER_MODE
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_FIFOModeEnable(FifoMode_t fm) {
 u8_t value;  
 
 if(fm == FIFO_DISABLE) {
    if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, FIFO_CTRL_REG_A, &value) )
     return MEMS_ERROR;  
   value &= 0x1f;
   value |= (FIFO_BYPASS_MODE<<FM);                    
   if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, FIFO_CTRL_REG_A, value) )           //fifo mode bypass
     return MEMS_ERROR;  
   
   if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, CTRL_REG5_A, &value) )
     return MEMS_ERROR;                
   value &= 0xBF;    
   if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, CTRL_REG5_A, value) )               //fifo disable
     return MEMS_ERROR;  
 }
 
 if(fm == FIFO_BYPASS_MODE)   {  
   if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, CTRL_REG5_A, &value) )
     return MEMS_ERROR;              
   value &= 0xBF;
   value |= MEMS_SET<<FIFO_EN;
   if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, CTRL_REG5_A, value) )               //fifo enable
     return MEMS_ERROR;  
   
   if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, FIFO_CTRL_REG_A, &value) )
     return MEMS_ERROR;  
   value &= 0x1f;
   value |= (fm<<FM);                     //fifo mode configuration  
   if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, FIFO_CTRL_REG_A, value) )
     return MEMS_ERROR;
 }
 
 if(fm == FIFO_MODE)   {
   if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, CTRL_REG5_A, &value) )
     return MEMS_ERROR;                
   value &= 0xBF;
   value |= MEMS_SET<<FIFO_EN;
   if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, CTRL_REG5_A, value) )               //fifo enable
     return MEMS_ERROR;  
   
   if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, FIFO_CTRL_REG_A, &value) )
     return MEMS_ERROR;
   value &= 0x1f;
   value |= (fm<<FM);                      //fifo mode configuration
   if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, FIFO_CTRL_REG_A, value) )
     return MEMS_ERROR;
 }
 
 if(fm == FIFO_STREAM_MODE)   {  
   if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, CTRL_REG5_A, &value) )
     return MEMS_ERROR;                
   value &= 0xBF;
   value |= MEMS_SET<<FIFO_EN;
   if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, CTRL_REG5_A, value) )               //fifo enable
     return MEMS_ERROR;  
   
   if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, FIFO_CTRL_REG_A, &value) )
     return MEMS_ERROR;  
   value &= 0x1f;
   value |= (fm<<FM);                      //fifo mode configuration  
   if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, FIFO_CTRL_REG_A, value) )
     return MEMS_ERROR;
 }
 
 if(fm == FIFO_TRIGGER_MODE)   {  
   if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, CTRL_REG5_A, &value) )
     return MEMS_ERROR;                
   value &= 0xBF;
   value |= MEMS_SET<<FIFO_EN;
   if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, CTRL_REG5_A, value) )               //fifo enable
     return MEMS_ERROR;    
 
   if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, FIFO_CTRL_REG_A, &value) )
     return MEMS_ERROR;
   value &= 0x1f;
   value |= (fm<<FM);                      //fifo mode configuration
   if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, FIFO_CTRL_REG_A, value) )
     return MEMS_ERROR;
 }
 return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : SetTriggerInt
* Description    : Trigger event liked to trigger signal INT1/INT2
* Input          : TRIG_INT1/TRIG_INT2
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_SetTriggerInt(TrigInt_t tr) {
 u8_t value;  
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, FIFO_CTRL_REG_A, &value) )
     return MEMS_ERROR;
   
   value &= 0xDF;
   value |= (tr<<TR);

 if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, FIFO_CTRL_REG_A, value) )
   return MEMS_ERROR;

 return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : SetWaterMark
* Description    : Sets Watermark Value
* Input          : Watermark = [0,31]
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_SetWaterMark(u8_t wtm) {
 u8_t value;
 
 if(wtm > 31)
   return MEMS_ERROR;  
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, FIFO_CTRL_REG_A, &value) )
   return MEMS_ERROR;
                 
 value &= 0xE0;
 value |= wtm;
 
 if( !LSM303DLHC_WriteReg(ACC_I2C_ADDRESS, FIFO_CTRL_REG_A, value) )
   return MEMS_ERROR;
 
 return MEMS_SUCCESS;
}

   
/*******************************************************************************
* Function Name  : GetSatusReg
* Description    : Read the status register
* Input          : char to empty by Status Reg Value
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_GetSatusReg(u8_t* val) {
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, STATUS_REG_A, val) )
     return MEMS_ERROR;
 
 return MEMS_SUCCESS;  
}

     
/*******************************************************************************
* Function Name  : GetSatusBIT
* Description    : Read the status register BIT
* Input          : STATUS_REG_ZYXOR, STATUS_REG_ZOR, STATUS_REG_YOR, STATUS_REG_XOR,
                  STATUS_REG_ZYXDA, STATUS_REG_ZDA, STATUS_REG_YDA, STATUS_REG_XDA, DATAREADY_BIT
* Output         : status register BIT
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_GetSatusBit(u8_t statusBIT) {
 u8_t value;  
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, STATUS_REG_A, &value) )
     return MEMS_ERROR;

 switch (statusBIT){
   case STATUS_REG_ZYXOR:     if(value &= STATUS_REG_ZYXOR) return MEMS_SUCCESS;
                              else  return MEMS_ERROR;
   case STATUS_REG_ZOR:       if(value &= STATUS_REG_ZOR) return MEMS_SUCCESS;
                              else  return MEMS_ERROR;
   case STATUS_REG_YOR:       if(value &= STATUS_REG_YOR) return MEMS_SUCCESS;
                              else  return MEMS_ERROR;                              
   case STATUS_REG_XOR:       if(value &= STATUS_REG_XOR) return MEMS_SUCCESS;
                              else  return MEMS_ERROR;  
   case STATUS_REG_ZYXDA:     if(value &= STATUS_REG_ZYXDA) return MEMS_SUCCESS;
                              else  return MEMS_ERROR;
   case STATUS_REG_ZDA:       if(value &= STATUS_REG_ZDA) return MEMS_SUCCESS;
                              else  return MEMS_ERROR;
   case STATUS_REG_YDA:       if(value &= STATUS_REG_YDA) return MEMS_SUCCESS;
                              else  return MEMS_ERROR;
   case STATUS_REG_XDA:       if(value &= STATUS_REG_XDA) return MEMS_SUCCESS;
                              else  return MEMS_ERROR;                                
   
 }
return MEMS_ERROR;
}

 
/*******************************************************************************
* Function Name  : GetAccAxesRaw
* Description    : Read the Acceleration Values Output Registers
* Input          : buffer to empty by AccAxesRaw_t Typedef
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_GetAccAxesRaw(AxesRaw_t* buff) {

	LSM303DLHC_ReadBuffer(MAG_I2C_ADDRESS, OUT_X_L_M, buff);

/* u8_t valueL;
 u8_t valueH;
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, OUT_X_L_A, &valueL) )
     return MEMS_ERROR;
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, OUT_X_H_A, &valueH) )
     return MEMS_ERROR;
 
 buff->AXIS_X = (i16_t)( (valueH << 8) | valueL )/16;
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, OUT_Y_L_A, &valueL) )
     return MEMS_ERROR;
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, OUT_Y_H_A, &valueH) )
     return MEMS_ERROR;
 
 buff->AXIS_Y = (i16_t)( (valueH << 8) | valueL )/16;
 
  if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, OUT_Z_L_A, &valueL) )
     return MEMS_ERROR;
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, OUT_Z_H_A, &valueH) )
     return MEMS_ERROR;
 
 buff->AXIS_Z = (i16_t)( (valueH << 8) | valueL )/16;*/
 
 return MEMS_SUCCESS;  
}

/*******************************************************************************
* Function Name  : LSM303DLHC_GetAccRate
* Description    : Read the Accelerometer Rate Registers.
* Input          : Accel pointer
* Output         : Accelerometer Rate
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_GetAccAxes(AxesRaw_t* buff) {

	//int StdDev = 0;

	LSM303DLHC_GetAccAxesRaw(buff);


	buff->AXIS_X = LSM303DLHC_ACC_SENSITIVITY_2 * (buff->AXIS_X - LSM303DLHC_ACC_ZERO_RATE);
	buff->AXIS_Y = LSM303DLHC_ACC_SENSITIVITY_2 * (buff->AXIS_Y - LSM303DLHC_ACC_ZERO_RATE);
	buff->AXIS_Z = LSM303DLHC_ACC_SENSITIVITY_2 * (buff->AXIS_Z - LSM303DLHC_ACC_ZERO_RATE);

	return MEMS_SUCCESS;

	//With Standard Deviation
	/*if(fabsf(buff->AXIS_X - L3GD20_ZERO_RATE_500) < StdDev)
		buff->AXIS_X = 0;
	else
		buff->AXIS_X = L3GD20_SENSITIVITY_500 * (buff->AXIS_X - L3GD20_ZERO_RATE_500);*/
}


/*******************************************************************************
* Function Name  : GetMagAxesRaw
* Description    : Read the Magnetometer Values Output Registers
* Input          : buffer to empty by MagAxesRaw_t Typedef
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_GetMagAxesRaw(AxesRaw_t* buff) {

	LSM303DLHC_ReadBuffer(MAG_I2C_ADDRESS, OUT_X_L_M, buff);

 /*u8_t valueL;
 u8_t valueH;
 
 if( !LSM303DLHC_ReadReg(MAG_I2C_ADDRESS, OUT_X_L_M, &valueL) )
     return MEMS_ERROR;
 
 if( !LSM303DLHC_ReadReg(MAG_I2C_ADDRESS, OUT_X_H_M, &valueH) )
     return MEMS_ERROR;
 
 buff->AXIS_X = (i16_t)( (valueH << 8) | valueL )/16;
 
 if( !LSM303DLHC_ReadReg(MAG_I2C_ADDRESS, OUT_Y_L_M, &valueL) )
     return MEMS_ERROR;
 
 if( !LSM303DLHC_ReadReg(MAG_I2C_ADDRESS, OUT_Y_H_M, &valueH) )
     return MEMS_ERROR;
 
 buff->AXIS_Y = (i16_t)( (valueH << 8) | valueL )/16;
 
  if( !LSM303DLHC_ReadReg(MAG_I2C_ADDRESS, OUT_Z_L_M, &valueL) )
     return MEMS_ERROR;
 
 if( !LSM303DLHC_ReadReg(MAG_I2C_ADDRESS, OUT_Z_H_M, &valueH) )
     return MEMS_ERROR;
 
 buff->AXIS_Z = (i16_t)( (valueH << 8) | valueL )/16;*/
 
 return MEMS_SUCCESS;  
}

/*******************************************************************************
* Function Name  : LSM303DLHC_GetMagAxes
* Description    : Read the Magnetometer Registers.
* Input          : Mag pointer
* Output         : Magnetometer
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_GetMagAxes(AxesRaw_t* buff) {

	//int StdDev = 0;

	LSM303DLHC_GetMagAxesRaw(buff);

	buff->AXIS_X = buff->AXIS_X / MagGain.XY;
	buff->AXIS_Y = buff->AXIS_Y / MagGain.XY;;
	buff->AXIS_Z = buff->AXIS_Z / MagGain.Z;;

	return MEMS_SUCCESS;

	//With Standard Deviation
	/*if(fabsf(buff->AXIS_X - L3GD20_ZERO_RATE_500) < StdDev)
		buff->AXIS_X = 0;
	else
		buff->AXIS_X = L3GD20_SENSITIVITY_500 * (buff->AXIS_X - L3GD20_ZERO_RATE_500);*/
}

/*******************************************************************************
* Function Name  : GetInt1Src
* Description    : Reset Interrupt 1 Latching function
* Input          : None
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_GetInt1Src(u8_t* val) {
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, INT1_SRC_A, val) )
   return MEMS_ERROR;
 
 return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : GetInt1SrcBit
* Description    : Reset Interrupt 1 Latching function
* Input          : INT1_SRC_IA, INT1_SRC_ZH, INT1_SRC_ZL, INT1_SRC_YH .....
* Output         : None
* Return         : Status of BIT [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_GetInt1SrcBit(u8_t statusBIT) {
 u8_t value;  
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, INT1_SRC_A, &value) )
     return MEMS_ERROR;

 if(statusBIT == INT_SRC_IA){
   if(value &= INT_SRC_IA)    return MEMS_SUCCESS;
   else  return MEMS_ERROR;  
 }
 
 if(statusBIT == INT_SRC_ZH){
   if(value &= INT_SRC_ZH)    return MEMS_SUCCESS;
   else  return MEMS_ERROR;  
 }

 if(statusBIT == INT_SRC_ZL){
   if(value &= INT_SRC_ZL)    return MEMS_SUCCESS;
   else  return MEMS_ERROR;  
 }
 
 if(statusBIT == INT_SRC_YH){
   if(value &= INT_SRC_YH)    return MEMS_SUCCESS;
   else  return MEMS_ERROR;  
 }

 if(statusBIT == INT_SRC_YL){
   if(value &= INT_SRC_YL)    return MEMS_SUCCESS;
   else  return MEMS_ERROR;  
 }

 if(statusBIT == INT_SRC_XH){
   if(value &= INT_SRC_XH)    return MEMS_SUCCESS;
   else  return MEMS_ERROR;  
 }

 if(statusBIT == INT_SRC_XL){
   if(value &= INT_SRC_XL)    return MEMS_SUCCESS;
   else  return MEMS_ERROR;  
 }
return MEMS_ERROR;
}


/*******************************************************************************
* Function Name  : GetInt2SrcBit
* Description    : Reset Interrupt 2 Latching function
* Input          : INT_SRC_IA, INT_SRC_ZH, INT_SRC_ZL, INT_SRC_YH .....
* Output         : None
* Return         : Status of BIT [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_GetInt2SrcBit(u8_t statusBIT) {
 u8_t value;  
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, INT2_SRC_A, &value) )
     return MEMS_ERROR;

 if(statusBIT == INT_SRC_IA){
   if(value &= INT_SRC_IA)    return MEMS_SUCCESS;
   else  return MEMS_ERROR;  
 }
 
 if(statusBIT == INT_SRC_ZH){
   if(value &= INT_SRC_ZH)    return MEMS_SUCCESS;
   else  return MEMS_ERROR;  
 }

 if(statusBIT == INT_SRC_ZL){
   if(value &= INT_SRC_ZL)    return MEMS_SUCCESS;
   else  return MEMS_ERROR;  
 }
 
 if(statusBIT == INT_SRC_YH){
   if(value &= INT_SRC_YH)    return MEMS_SUCCESS;
   else  return MEMS_ERROR;  
 }

 if(statusBIT == INT_SRC_YL){
   if(value &= INT_SRC_YL)    return MEMS_SUCCESS;
   else  return MEMS_ERROR;  
 }

 if(statusBIT == INT_SRC_XH){
   if(value &= INT_SRC_XH)    return MEMS_SUCCESS;
   else  return MEMS_ERROR;  
 }

 if(statusBIT == INT_SRC_XL){
   if(value &= INT_SRC_XL)    return MEMS_SUCCESS;
   else  return MEMS_ERROR;  
 }
return MEMS_ERROR;
}


/*******************************************************************************
* Function Name  : GetFifoSourceReg
* Description    : Read Fifo source Register
* Input          : Byte to empty by FIFO source register value
* Output         : None
* Return         : Status [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_GetFifoSourceReg(u8_t* val) {
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, FIFO_SRC_REG_A, val) )
   return MEMS_ERROR;
 
 return MEMS_SUCCESS;
}


/*******************************************************************************
* Function Name  : GetFifoSourceBit
* Description    : Read Fifo WaterMark source bit
* Input          : FIFO_SRC_WTM, FIFO_SRC_OVRUN, FIFO_SRC_EMPTY
* Output         : None
* Return         : Status of BIT [MEMS_ERROR, MEMS_SUCCESS]
*******************************************************************************/
status_t LSM303DLHC_GetFifoSourceBit(u8_t statusBIT){
 u8_t value;  
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, FIFO_SRC_REG_A, &value) )
     return MEMS_ERROR;

 if(statusBIT == FIFO_SRC_WTM){
   if(value &= FIFO_SRC_WTM)     return MEMS_SUCCESS;
   else  return MEMS_ERROR;  
 }
 
 if(statusBIT == FIFO_SRC_OVRUN){
   if(value &= FIFO_SRC_OVRUN)   return MEMS_SUCCESS;
   else  return MEMS_ERROR;  
 }

 if(statusBIT == FIFO_SRC_EMPTY){
   if(value &= FIFO_SRC_EMPTY)   return MEMS_SUCCESS;
   else  return MEMS_ERROR;  
 }
return MEMS_ERROR;
}


/*******************************************************************************
* Function Name  : GetFifoSourceFSS
* Description    : Read current number of unread samples stored in FIFO
* Input          : Byte to empty by FIFO unread sample value
* Output         : None
* Return         : Status [value of FSS]
*******************************************************************************/
status_t LSM303DLHC_GetFifoSourceFSS(u8_t* val){
 u8_t value;
 
 if( !LSM303DLHC_ReadReg(ACC_I2C_ADDRESS, FIFO_SRC_REG_A, &value) )
   return MEMS_ERROR;

 value &= 0x1F;
 *val = value;
 
 return MEMS_SUCCESS;
}
